
#include <agv_peripheral_controller/roller/io_lift.h>
#include <algorithm>
#include <thread>
#include <chrono>
#include <future>
namespace peripheral_roller
{

    ////////////////////控制输出io工具类////////////////////
    COutputIoTool::COutputIoTool()
    {
    }
    // 初始化客户端
    void COutputIoTool::init(ros::ServiceClient &client)
    {
        m_client = client;
    }

    bool COutputIoTool::set(IO_GROUP outIoGroup)
    {
        // 初始化是否完成
        bool ret = false;

        // 客户端是否有效
        if (!m_client.isValid())
        {
            DEBUG_ERROR_OUT("CIoLift:outputinterface. client is not valid");
            return ret;
        }

        if (m_outIoRecord == outIoGroup)
        {
            // DEBUG__OUT("CIoLift:outIoGroup is the same as last time");
            return true;
        }

        agv_srvs::srv::OutputsNew srv;
        srv.request.array = outIoGroup;
        if (m_client.call(srv) && srv.response.state)
        {
            ret = true;
            m_outIoRecord = outIoGroup;
            DEBUG_OUT("CIoLift:control io sucess ;");
        }
        else
        {
            ret = false;
            DEBUG_ERROR_OUT("CIoLift: control io fail;");
        }

        return ret;
    }

    ///////////////////////////单举升类/////////////////////////////
    CIoLift::CIoLift()
    {
        resetFlag = false;
        m_futures.clear();
    }

    CIoLift::~CIoLift()
    {
    }

    bool CIoLift::init(ros::ServiceClient &client)
    {
        m_outputIoTool.init(client);
        return true;
    }

    // 举升上升
    bool CIoLift::up()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }
        // 如果到达上限位，停止完成此动作
        if (upTarget())
        {
            finish = stop();
            DEBUG_WARN_OUT("CIoLift:up target:key=" << ctrl[0].key
                                                    << ",set stop finish=" << finish);
            return finish;
        }

        // 控制上升输出

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = true;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = m_outputIoTool.set(ctrl);
        DEBUG_OUT("up:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升下降
    bool CIoLift::down()
    {
        bool finish = false;
        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 如果到达下限位，停止完成此动作
        if (downTarget())
        {
            finish = stop();
            DEBUG_WARN_OUT("CIoLift:down target:key=" << ctrl[0].key
                                                      << ",set stop finish=" << finish);
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = true;
        }
        else
        {
            ctrl[0].value = true;
        }
        bool ret = m_outputIoTool.set(ctrl);
        DEBUG_OUT("down:set io out key=" << ctrl[0].key << ",ret=" << ret);
        return finish;
    }

    // 举升停止
    bool CIoLift::stop()
    {
        bool finish = false;

        if (ctrl.size() == 0 || ctrl.size() > 2)
        {
            return finish;
        }

        // 控制停止输出
        if (ctrl.size() == 2)
        {
            ctrl[0].value = false;
            ctrl[1].value = false;
        }
        else
        {
            ctrl[0].value = false;
        }
        finish = m_outputIoTool.set(ctrl);

        if (finish)
        {
            DEBUG_WARN_OUT("stop lift ok");
        }
        return finish;
    }

    // 货物装填
    bool CIoLift::loadStatus()
    {
        bool load = false;
        // 如果不处在下限位，则一直报有货状态
        bool unload = downTarget() && !upTarget();
        load = !unload;
        return load;
    }

    // 上升到位
    bool CIoLift::upTarget()
    {
        bool ret = inputIo(upLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("upTarget ok");
        }
        return ret;
    }

    // 下降到位
    bool CIoLift::downTarget()
    {
        bool ret = inputIo(downLimit);
        if (ret)
        {
            DEBUG_WARN_OUT("downTarget ok");
        }
        return ret;
    }

    // 清除一次错误
    bool CIoLift::clearErrorOnce()
    {
        if (!hasError())
        {
            DEBUG_OUT("CIoMotorSignal don not need error");
            return false;
        }

        if (clearFaultOut.size() == 0)
        {
            DEBUG_OUT("CIoMotorSignal clearFaultOut.size is 0");
            return false;
        }

        if (resetFlag)
        {
            DEBUG_WARN_OUT("resetFlag is true;currently resetting....");
            return false;
        }

        // 清理
        m_futures.remove_if([](const std::future<void> &f)
                            {
			// return true if callback has finished it's computation.
			return (f.wait_for(std::chrono::steady_clock::duration::zero())==std::future_status::ready); });

        auto future = std::async(std::launch::async, [this]()
                                 {
                                 resetFlag = true;
                                 for (auto &io : clearFaultOut)
                                 {
                                     io.value = true;
                                 }
                                 m_outputIoTool.set(clearFaultOut);
                                 DEBUG_WARN_OUT("resetFlag true,wait 300ms...");
                                 // 暂停 300 毫秒  
                                std::this_thread::sleep_for(std::chrono::milliseconds(300));  
                                 for (auto &io : clearFaultOut)
                                 {
                                     io.value = false;
                                 }
                                 m_outputIoTool.set(clearFaultOut);
                                 resetFlag = false;
                                 DEBUG_WARN_OUT("resetFlag false ok"); });

        m_futures.push_front(std::move(future));

        return true;
    }

    // 电机是否触发了错误
    bool CIoLift::hasError()
    {
        bool ret = inputIo(errorIn);
        if (ret)
        {
            DEBUG_ERROR_OUT("error:IoMotorSignal occur:name" << errorIn.Get().key);
        }
        return ret;
    }
}; // end namespace
